#include <motor/servo_keya.h>

namespace ros_canopen_driver {

ServoKeya::ServoKeya(std::shared_ptr<rclcpp::Node> node, const std::string name)
{
    node_ = node;
    joint_name = name;
    servo_manufacturer = KEYA;
    memset(&m_servo_motor, 0, sizeof(SERVOMOTOR_PORT));
    motorState = Off;
    pc_.timestamp = node_->now().seconds();
}

ServoKeya::~ServoKeya()
{
}

/*Description: load motor configuration parameters from yaml file*/
bool ServoKeya::Motor_InitParam()
{
    if (!node_->has_parameter(joint_name + "/nodeId"))
    {
        RCLCPP_ERROR_STREAM(node_->get_logger(), joint_name << " nodeId is invaild");
        return false;
    }
    int nodeId;
    node_->get_parameter(joint_name + "/nodeId", nodeId);
    m_servo_motor.cfg.nodeId = nodeId;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" nodeId is " << nodeId);

    node_->get_parameter_or(joint_name + "/busname", busname, std::string("can0"));
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" busname: "<<busname);
    
    node_->get_parameter_or(joint_name + "/type", servo_type, 0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" type: "<<servo_type);

    node_->get_parameter_or(joint_name + "/ratio", ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" ratio: "<<ratio);

    node_->get_parameter_or(joint_name + "/pos_scale", pos_scale, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" pos_scale: "<<pos_scale);

    node_->get_parameter_or(joint_name + "/pos_offset", pos_offset, 0.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" pos_offset: "<<pos_offset);

    node_->get_parameter_or(joint_name + "/use_extra_encoder", use_extra_encoder, false);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" use external encoder: "<<use_extra_encoder);

    node_->get_parameter_or(joint_name + "/rad_to_meter", rad_to_meter, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" rad_to_meter: "<<rad_to_meter);

    node_->get_parameter_or(joint_name + "/zero_point_correction", zero_point_correction, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" zero_point_correction: "<<zero_point_correction);
    /* <--- add end */

    int ctl_mode;
    node_->get_parameter_or(joint_name + "/ctl_mode", ctl_mode, 0);
    m_servo_motor.cfg.ctl_mode = ctl_mode;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" ctl_mode is "<< ctl_mode);

    node_->get_parameter_or(joint_name + "/rotateReverse", m_servo_motor.cfg.rotateReverse, false);

    node_->get_parameter_or(joint_name + "/reduction_ratio", m_servo_motor.cfg.reduction_ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" reduction_ratio is "<< m_servo_motor.cfg.reduction_ratio);

    node_->get_parameter_or(joint_name + "/encoder_resolution", m_servo_motor.cfg.encoder_resolution, 10000.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" encoder_resolution is "<< m_servo_motor.cfg.encoder_resolution);        

    // position ctl mode cfg
    int position_mode_vel;
    node_->get_parameter_or(joint_name + "/pModeCfg/vel", position_mode_vel, 0);
    m_servo_motor.cfg.pModeCfg.vel = position_mode_vel;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" position_mode_vel is "<< m_servo_motor.cfg.pModeCfg.vel);

    //home cfg
    node_->get_parameter_or(joint_name + "/homeNeed", m_servo_motor.cfg.homeNeed, false);
    if (m_servo_motor.cfg.homeNeed)
    {
        int home_mode;
        node_->get_parameter_or(joint_name + "/hModeCfg/home_mode", home_mode, 0);
        m_servo_motor.cfg.hModeCfg.home_mode = home_mode;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" home_mode is "<< home_mode);

        int home_mode_velH;
        node_->get_parameter_or(joint_name + "/hModeCfg/velH", home_mode_velH, 0);
        m_servo_motor.cfg.hModeCfg.velH = home_mode_velH;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" home_mode_velH is "<< m_servo_motor.cfg.hModeCfg.velH);

        int home_mode_velL;
        node_->get_parameter_or(joint_name + "/hModeCfg/velL", home_mode_velL, 0);
        m_servo_motor.cfg.hModeCfg.velL = home_mode_velL;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" home_mode_velL is "<< m_servo_motor.cfg.hModeCfg.velL);
    }
   
    return true;
}

/*Description: motor init(include motor enable,gohome and switch ctlmode)*/
int ServoKeya::Motor_Init()
{
    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if((0 == nodeId) || (nodeId > 127))
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]motor nodeId is unvaild");
        return -1;       
    }

    if (servo_type == KEYA_A) {
        set_vel_pub           = node_->create_publisher<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/set_the_data_of_motor_a", 10);
        set_pos_pub           = node_->create_publisher<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/set_the_data_of_motor_a", 10);
        set_control_word_pub  = node_->create_publisher<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/set_enable_motor_a", 10);
    
        current_pos_sub  = node_->create_subscription<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motora_position", 
            10, 
            std::bind(&ServoKeya::current_pos_sub_callback, this, std::placeholders::_1));
        current_vel_sub  = node_->create_subscription<std_msgs::msg::Int16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motora_speed", 
            10, 
            std::bind(&ServoKeya::current_vel_sub_callback, this, std::placeholders::_1));
        current_sub      = node_->create_subscription<std_msgs::msg::Int16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motora_current", 
            10, 
            std::bind(&ServoKeya::current_sub_callback, this, std::placeholders::_1));
        errcode_sub      = node_->create_subscription<std_msgs::msg::UInt16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motora_close", 
            10, 
            std::bind(&ServoKeya::errcode_sub_callback, this, std::placeholders::_1));
    }
    else if (servo_type == KEYA_B) {
        set_vel_pub           = node_->create_publisher<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/set_the_data_of_motor_b", 10);
        set_pos_pub           = node_->create_publisher<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/set_the_data_of_motor_b", 10);
        set_control_word_pub  = node_->create_publisher<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/set_enable_motor_b", 10);
        
        current_pos_sub  = node_->create_subscription<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motorb_position", 
            10, 
            std::bind(&ServoKeya::current_pos_sub_callback, this, std::placeholders::_1));
        current_vel_sub  = node_->create_subscription<std_msgs::msg::Int16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motorb_speed", 
            10, 
            std::bind(&ServoKeya::current_vel_sub_callback, this, std::placeholders::_1));
        current_sub      = node_->create_subscription<std_msgs::msg::Int16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motorb_current", 
            10, 
            std::bind(&ServoKeya::current_sub_callback, this, std::placeholders::_1));
        errcode_sub      = node_->create_subscription<std_msgs::msg::UInt16>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_motorb_close", 
            10, 
            std::bind(&ServoKeya::errcode_sub_callback, this, std::placeholders::_1));
    }

    status_word_sub  = node_->create_subscription<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_motorstate", 
        10, 
        std::bind(&ServoKeya::status_word_sub_callback, this, std::placeholders::_1));

    set_entry_cln    = node_->create_client<agv_srvs::srv::SetCanopenEntry>("/set_canopen_entry");
    RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] %s subscribe prefix: %s", 
        joint_name.c_str(), std::string("/" + busname + "/device" + std::to_string(nodeId)).c_str());

    RCLCPP_INFO(node_->get_logger(), "node[%d] enable.", nodeId);
    // motor enable
    bool enableFlage = Motor_Enable(true);
    if(!enableFlage)
    {
        return -1;
    }

    // homemachine
    Motor_HomeMachine();

    // ctlmode
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set position mode.", nodeId);
        if (!Motor_InitPmode())
            return -1;
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set velocity mode.", nodeId);
        if(!Motor_InitVmode())
            return -1;
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]unknown drive mode[%d]", m_servo_motor.cfg.ctl_mode);
        return -1;
    }
    
    while (!set_entry_cln->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] Interrupted while waiting for the service. Exiting.");
        return -1;
      }
      RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] service not available, waiting again...");
    }      

    motorState = Ready;  
    return 0;
}

bool ServoKeya::servo_isEnable()
{
    uint32_t statusWord = pc_.status_word;
    bool enable = false;
    if (servo_type == KEYA_A) {
        enable = (((statusWord >> 2) & 0x1) == 1);
    }
    else if (servo_type == KEYA_B) {
        enable =  (((statusWord >> 3) & 0x1) == 1);
    }
    else {
        enable = false;
    }
    
    RCLCPP_INFO(node_->get_logger(), "[ServoKeya] %s enable status [%d]", joint_name.c_str(), enable);
    return enable;
}

bool ServoKeya::servo_isModeSet()
{
    uint8_t mode = pc_.mode;
    RCLCPP_INFO(node_->get_logger(), "[ServoKeya] %s current mode: %d  target: %d", joint_name.c_str(), mode, m_servo_motor.cfg.ctl_mode);
    return mode == m_servo_motor.cfg.ctl_mode;
    // if (m_servo_motor.cfg.ctl_mode == Profiled_Velocity) {
    //     RCLCPP_INFO(node_->get_logger(), "[ServoKeya] target mode: %d  current: %d", m)
    //     return (mode == Profiled_Velocity);
    // }
    // else if (m_servo_motor.cfg.ctl_mode == Profiled_Position) {
    //     return (mode == Profiled_Position);
    // }
}


/*Description: motor enable or disable*/
bool ServoKeya::Motor_Enable(bool onFlag)
{
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        std_msgs::msg::Int32 msg;
        msg.data = (int32_t)0x00;
        set_pos_pub->publish(msg);
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity || m_servo_motor.cfg.ctl_mode == Immediate_Velocity)
    {
        Motor_SetTargetVelEnc(0);
    }

    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    uint32_t statusWord = pc_.status_word;
    /* <--- modify end */


    RCLCPP_INFO(node_->get_logger(), "node %d statusWord is 0x%x", nodeId, statusWord);
    if(!onFlag)
    {

        if (servo_isEnable() == false)
            return true;;

        int timeout = 0;
        while (servo_isEnable() && timeout < 10) {
            std_msgs::msg::Int32 msg;
            if (servo_type == KEYA_A)
                msg.data = (int32_t)KEYA_DISABLE_A;
            else if (servo_type == KEYA_B)
                msg.data = (int32_t)KEYA_DISABLE_B;

            set_control_word_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
            statusWord = pc_.status_word;
            RCLCPP_INFO(node_->get_logger(), "[ServoKeya] checkout status_word[0x%x] trys: %d", statusWord, timeout);
            timeout++;
        } 

        if (servo_isEnable() == false)
            return true;
        else
            return false;
    }
    else
    {
        if(servo_isEnable() && servo_isModeSet()) {
            RCLCPP_INFO(node_->get_logger(), "[ServoKeya] motor has been enabled");
            return true;
        }

        uint16_t errcode = pc_.error_code;

        std_msgs::msg::Int32 msg;
        if (servo_type == KEYA_A)
            msg.data = (int32_t)KEYA_RESET_ERR_A;
        else if (servo_type == KEYA_B)
            msg.data = (int32_t)KEYA_RESET_ERR_B;
        set_control_word_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(20));
        
        int timeout = 0;
        while (errcode && timeout < 50) {
            if (servo_type == KEYA_A)
                msg.data = (int32_t)KEYA_RESET_ERR_A;
            else if (servo_type == KEYA_B)
                msg.data = (int32_t)KEYA_RESET_ERR_B;
            set_control_word_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            errcode = pc_.error_code;
            RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] 0x%x checkout error_code[0x%x] trys: %d", joint_name.c_str(), msg.data, errcode, timeout);
            timeout++;
        }

        if(!errcode)
        {
            uint32_t statusWord = pc_.status_word;
                
            timeout = 0;
            
            if (servo_type == KEYA_A) {
                if (m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
                    msg.data = (int32_t)KEYA_ENABLE_VEL_A;
                else if (m_servo_motor.cfg.ctl_mode == Profiled_Position)
                    msg.data = (int32_t)KEYA_ENABLE_ABSPOS_A;
                else {
                    RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] unsupportted mode %d", m_servo_motor.cfg.ctl_mode);
                    return false;
                }
            }
            else if (servo_type == KEYA_B) {
                if (m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
                    msg.data = (int32_t)KEYA_ENABLE_VEL_B;
                else if (m_servo_motor.cfg.ctl_mode == Profiled_Position)
                    msg.data = (int32_t)KEYA_ENABLE_ABSPOS_B;
                else {
                    RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] unsupportted mode %d", m_servo_motor.cfg.ctl_mode);
                    return false;
                }
            }

            set_control_word_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            statusWord = pc_.status_word;

            while ((servo_isEnable() == false) || (servo_isModeSet() == false) && timeout < 50) {
                set_control_word_pub->publish(msg);
                rclcpp::sleep_for(std::chrono::milliseconds(20));
                statusWord = pc_.status_word;
                RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] 0x%x checkout status_word[0x%x] trys: %d", joint_name.c_str(), msg.data, statusWord, timeout);
                timeout++;
            }

            if (servo_isEnable() && servo_isModeSet()) {
                RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] enable succeed.", joint_name.c_str());
                return true;
            }
            else {
                RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] servo[%s] init failed.", joint_name.c_str());   
                return false;
            }
        }
        else
        {
            RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] servo[%s] error[0x%x] is sitll obtained after clear error", joint_name.c_str(), errcode);
            return false;
        }
    }

    return false; 
}


/*Description: configure motor homing parameters and excute*/
bool ServoKeya::Motor_HomeMachine()
{
    return true;
}

/*Description: configure the motor for Profiled Velocity mode*/
bool ServoKeya::Motor_InitVmode()
{
    uint8_t mode = pc_.mode;
    return (mode == Profiled_Velocity);
}

/*Description: configure the motor for Profiled Position mode*/
bool ServoKeya::Motor_InitPmode()
{   
    uint8_t mode = pc_.mode;
    return (mode == Profiled_Position);
}

/*Description: switch motor mode, currently only supported Profiled Position mode and Profiled Velocity mode*/
bool ServoKeya::Motor_SwitchMode(int8_t mode)
{
    switch(mode)
    {
        case Profiled_Position:
            Motor_InitPmode();
            break;
        case Profiled_Velocity:
            Motor_InitVmode();
            break;
        case Homing:
            break;
        default:
            RCLCPP_WARN(node_->get_logger(), "servo[%s] undefined control mode %d", joint_name.c_str(), mode);
            break;
    }
    int8_t actualCtlMode = Motor_GetMode();
    if(actualCtlMode != mode)
    {
        RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] servo[%s] failed to set the ctl mode", joint_name.c_str());
        return false;
    }
    return true;
}

/*Description: set motor acceleration*/
void ServoKeya::Motor_SetAcc(uint32_t acc)
{
    return;
}

/*Description: set motor deceleration*/
void ServoKeya::Motor_SetDec(uint32_t dec)
{
    return;
}

/*Description: set the speed for Profiled Position mode*/
void ServoKeya::Motor_SetPmodeVel(uint32_t vel)
{
    auto request = std::make_shared<agv_srvs::srv::SetCanopenEntry::Request>();
    request->bus_name = std::string("can0");
    request->node_id = m_servo_motor.cfg.nodeId;
    request->index = 0x6099;
    if (servo_type == KEYA_A)
        request->sub_index = 0x1;
    else if (servo_type == KEYA_B)
        request->sub_index = 0x2;
    request->value = vel;

    try {
        using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::SetCanopenEntry>::SharedFuture;
        auto response_received_callback = [this](ServiceResponseFuture future) {
            RCLCPP_INFO_STREAM(node_->get_logger(), 
                "[ServoKeya] chassis controller service success.");
        };

        auto result = set_entry_cln->async_send_request(request, response_received_callback);
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] call chassis controller service error");
    }

    RCLCPP_INFO(node_->get_logger(), "[ServoKeya] set position mode vel: %d", vel);
}

/*Description: set the speed for Profiled Velocity mode, vel is expressed in rad/s*/
void ServoKeya::Motor_SetTargetVelRad(double vel)
{
    /* 2023.8.17 add begin: add motor state condition  ---> */
    if(Ready != Motor_GetState())
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKeya] servo[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
        return;
    }
    /* <--- add end */

    if(Motor_GetMode() != Profiled_Velocity && Motor_GetMode() != Immediate_Velocity)
    {
        return;
    } 
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKeya] servo[%s] reduction_ratio||encoder_resolution is 0.", joint_name.c_str());
        return;
    }

    if (std::isnan(vel) || abs(vel) > 1e9) {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKeya] bad vel value: %.4f", vel);
        Motor_SetTargetVelEnc(0);
        return;
    }
    
    //RCLCPP_INFO(node_->get_logger(), "vel is %lf",vel);

    /* 2023.8.17 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        vel /= rad_to_meter;
    } else {
        // continuous: do nothing
    }

    /* <--- and end */
    // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 0.5, "[ServoKeya] set target vel rad: %.4f", vel);
    // const double temp  = 30 / M_PI * m_servo_motor.cfg.reduction_ratio * 512 * m_servo_motor.cfg.encoder_resolution  / 1875;
    const double temp = 30 / M_PI * m_servo_motor.cfg.reduction_ratio * m_servo_motor.cfg.encoder_resolution / 1500;
    int targetVel =  vel * temp;
    if (targetVel > 10000)
        targetVel = 10000;
    else if (targetVel < -10000)
        targetVel = -10000;

    /* 2023.8.17 modify begin:  use setTargetVelEnc instead  --->
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_60FF00, targetVel);
    <--- old code  |  new code  ---> */
    Motor_SetTargetVelEnc(targetVel);
    /* <--- modify end */
}

/*Description: set the speed for Profiled Velocity mode, vel is expressed in encoder*/
void ServoKeya::Motor_SetTargetVelEnc(int vel)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_60FF00, vel);
    */
    RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
        500, "[ServoKeya] servo[%s] set_target vel enc: %d", joint_name.c_str(), vel);
    std_msgs::msg::Int32 msg;
    msg.data = vel;
    set_vel_pub->publish(msg);
    /* <--- modify end */
}

/*Description: set the target pos for Absolute position control mode, pos is expressed in encoder*/
void ServoKeya::Motor_SetTargetPosEnc(int pos)
{
    if(Ready != Motor_GetState())
    {
        return;
    }
    int encoder = servo_encoderGet();
    // RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]get encoder %d",encoder);
    if(abs(pos - encoder) < 100)
    {
        return;
    }

    std_msgs::msg::Int32 pos_msg;
    pos_msg.data = pos;
    set_pos_pub->publish(pos_msg);
}

/*Description: set the target pos for Absolute position control mode, pos is expressed in rad*/
void ServoKeya::Motor_SetTargetPosRad(double pos)
{
    if(Ready != Motor_GetState())
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKeya] servo[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
        return;
    }
    if(Motor_GetMode() != Profiled_Position)
    {
        // RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] motor mode[%d] not Profiled_Position !!!", Motor_GetMode());
        return;
    }
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKeya] servo[%s] reduction_ratio = 0 || encoder_resolution = 0", joint_name.c_str());
        return;
    }

     /* 2023.8.17 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        pos /= rad_to_meter;
    } else {
        // continuous: do nothing
    }

    /* <--- and end */


    //int targetEnc = pos / 2 / M_PI * m_servo_motor.cfg.encoder_resolution * m_servo_motor.cfg.reduction_ratio;
    int encoder = servo_encoderGet();
    double radToEnc = 2.0 * M_PI / m_servo_motor.cfg.encoder_resolution / m_servo_motor.cfg.reduction_ratio;
    int targetEnc = 0;
    if (servo_type == STEER) {
        targetEnc = pos /radToEnc + zero_point_correction;
        // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 1000, "[ServoKeya] %s set pos[%.4f]  enc[%d]  current[%d]", joint_name.c_str(), pos, targetEnc, encoder);
        Motor_SetTargetPosEnc(targetEnc);
    }
    else {
        if (abs(radToEnc) > 1e-6)
            targetEnc = pos / radToEnc;
        else 
            RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] radToEnc zero: %f", joint_name.c_str(), radToEnc);
    }

    // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 0.5, "[SERVO_MOTOR] get encoder: %d  targetEnc: %d",encoder, targetEnc);
    if(abs(targetEnc - encoder) < 100)
    {
        return;
    }
    Motor_SetTargetPosEnc(targetEnc);
}


/*Description: get the motor speed ignore control mode, pos is expressed in rad per second*/
double ServoKeya::Motor_GetCurrentVel()
{
    double retVel;

    retVel = (double)pc_.current_vel * M_PI / (30.0 * m_servo_motor.cfg.reduction_ratio);

    return retVel;
}


/*Description: get the motor position,unit is rad*/
double ServoKeya::Motor_GetCurrentPos()
{
    int encoder = servo_encoderGet();
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        return 0;
    }
    const double encTorad = m_servo_motor.cfg.encoder_resolution * m_servo_motor.cfg.reduction_ratio / 2.0 / M_PI;
    double pos = encoder / encTorad;

    /* 2023.8.18 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        pos *= rad_to_meter;
    } 
    else if (servo_type == STEER) {
        pos = (encoder - zero_point_correction) / encTorad;
    }
    else {
        // continuous: do nothing
    }

    pc_.timestamp = node_->now().seconds();

    return pos;
}

/*Description: get the motor encoder*/
int ServoKeya::servo_encoderGet()
{
    return pc_.current_pos;
}

/*Description: get the motor status word*/
uint16_t ServoKeya::Motor_GetStatusWord()
{
    return pc_.status_word;
}

std::string ServoKeya::Motor_Err2userErr(const uint16_t& motorErr)
{
    std::string errInfo;

    if (motorErr == 0 || motorErr == 1) {
        errInfo = "No error";
    }
    else {
        for (int i=0; i<16; i++) {
            if ((motorErr >> i) & 0x1 == 1) {
                switch (i+1) {
                    case  1: errInfo = "Not enable"; break;
                    case  2: errInfo = "Voltage too high"; break;
                    case  3: errInfo = "Current too high. hardware protect"; break;
                    case  4: errInfo = "EEPROM error"; break;
                    case  5: errInfo = "Voltage too low"; break;
                    case  6: errInfo = "Brake not released"; break;
                    case  7: errInfo = "Current too high. software protect"; break;
                    case  8: errInfo = "Bad control mode"; break;
                    case  9: errInfo = "UVW line lost"; break;
                    case 10: errInfo = "Mix mode some output error"; break;
                    case 11: errInfo = "Driver temperature over 85"; break;
                    case 12: errInfo = "Hall sensor error"; break;
                    case 13: errInfo = "Current sensor error"; break;
                    case 14: errInfo = "Motor temperature over 120"; break;
                    case 15: errInfo = "CAN communication lost"; break;
                    case 16: errInfo = "232 communication lost"; break;    
                    default: errInfo = "unknown warning"; break;
                }
            }
        }
    }


    return errInfo;
}

/*Description: get the motor error code from object 603f*/
uint16_t ServoKeya::Motor_GetErrCode()
{ 
    /* 2023.9.19  modify: canopen refactor.   --->
    uint16_t errCode = errCode = pCanopen->getErrCode(m_servo_motor.cfg.nodeId);
    */
    uint16_t errCode = pc_.error_code;

    /* <--- modify end */
    if(Ready == Motor_GetState())
    {
        if(errCode)
        {
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()),
                1000, "[ServoKeya] errcode: %d", errCode);
            motorState = Error;
        }
    }
    else {
        if (Error == Motor_GetState() && errCode == 0) {
            motorState = Ready;
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()),
                1000, "[ServoKeya] errcode clear");   
        }
    }

    return errCode;
}


/*Description: motor electric brake control,only zapi motor are supported*/
void ServoKeya::Motor_Brake(bool on)
{
    return;
}

/*Description: shutdown servo*/
void ServoKeya::Motor_Shutdown()
{
    return;
}

bool ServoKeya::Motor_Halt()
{
    motorState = Halt;
    std_msgs::msg::Int32 msg;
    if (servo_type == KEYA_A)
        msg.data = (int32_t)KEYA_DISABLE_A;
    else if (servo_type == KEYA_B)
        msg.data = (int32_t)KEYA_DISABLE_B;

    set_control_word_pub->publish(msg);
    rclcpp::sleep_for(std::chrono::milliseconds(20));

    if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity || m_servo_motor.cfg.ctl_mode == Immediate_Velocity) {
        Motor_SetTargetVelEnc(0);
    }

    int timeout = 0;
    uint32_t statusWord = 0;
    while (servo_isEnable() && timeout < 50) {
        std_msgs::msg::Int32 msg;
        if (servo_type == KEYA_A)
            msg.data = (int32_t)KEYA_DISABLE_A;
        else if (servo_type == KEYA_B)
            msg.data = (int32_t)KEYA_DISABLE_B;

        set_control_word_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(20));
        statusWord = pc_.status_word;
        RCLCPP_INFO(node_->get_logger(), "[ServoKeya] checkout status_word[0x%x] trys: %d", statusWord, timeout);
        timeout++;
    }    

    if (servo_isEnable() == false) {
        RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] disable succeed.", joint_name.c_str());
        return true;
    }
    else {
        RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] disable failed.", joint_name.c_str());
        return false;
    }
}

/*Description: reset servo error and enable servo*/
bool ServoKeya::Motor_Recover()
{
    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if (motorState != Recover)
    {
        motorState = Recover;

        if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity || m_servo_motor.cfg.ctl_mode == Immediate_Velocity) {
            Motor_SetTargetVelEnc(0);
        }
        
        uint16_t errcode = pc_.error_code;

        int timeout = 0;
        std_msgs::msg::Int32 msg;
        while (servo_isEnable() && timeout < 50) {
            if (servo_type == KEYA_A)
                msg.data = (int32_t)KEYA_DISABLE_A;
            else if (servo_type == KEYA_B)
                msg.data = (int32_t)KEYA_DISABLE_B;
            set_control_word_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            timeout++;
            RCLCPP_INFO(node_->get_logger(), 
                "[ServoKeya] servo[%s] 0x%x disable trys: %d", joint_name.c_str(), msg.data, errcode, timeout);
        }

        if (servo_isEnable()) {
            RCLCPP_ERROR(node_->get_logger(), 
                "[ServoKeya] servo[%s] disable failed", joint_name.c_str());
            motorState = Error;
            return false;
        }

        if (servo_type == KEYA_A)
            msg.data = (int32_t)KEYA_RESET_ERR_A;
        else if (servo_type == KEYA_B)
            msg.data = (int32_t)KEYA_RESET_ERR_B;
        set_control_word_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(20));
        
        timeout = 0;
        while (errcode && timeout < 50) {
            if (servo_type == KEYA_A)
                msg.data = (int32_t)KEYA_RESET_ERR_A;
            else if (servo_type == KEYA_B)
                msg.data = (int32_t)KEYA_RESET_ERR_B;
            set_control_word_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            errcode = pc_.error_code;
            RCLCPP_INFO(node_->get_logger(), 
                "[ServoKeya] servo[%s] 0x%x checkout error_code[0x%x] trys: %d", joint_name.c_str(), msg.data, errcode, timeout);
            timeout++;
        }

        if(!errcode)
        {
            uint32_t statusWord = pc_.status_word;
                            
            if (servo_type == KEYA_A) {
                if (m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
                    msg.data = (int32_t)KEYA_ENABLE_VEL_A;
                else if (m_servo_motor.cfg.ctl_mode == Profiled_Position)
                    msg.data = (int32_t)KEYA_ENABLE_ABSPOS_A;
                else {
                    RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] unsupportted mode %d", m_servo_motor.cfg.ctl_mode);
                    return false;
                }
            }
            else if (servo_type == KEYA_B) {
                if (m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
                    msg.data = (int32_t)KEYA_ENABLE_VEL_B;
                else if (m_servo_motor.cfg.ctl_mode == Profiled_Position)
                    msg.data = (int32_t)KEYA_ENABLE_ABSPOS_B;
                else {
                    RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] unsupportted mode %d", m_servo_motor.cfg.ctl_mode);
                    return false;
                }
            }

            set_control_word_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            statusWord = pc_.status_word;

            timeout = 0;

            while ((servo_isEnable() == false) || (servo_isModeSet() == false) && timeout < 50) {
                set_control_word_pub->publish(msg);
                rclcpp::sleep_for(std::chrono::milliseconds(20));
                statusWord = pc_.status_word;
                RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] 0x%x checkout status_word[0x%x] trys: %d", joint_name.c_str(), msg.data, statusWord, timeout);
                timeout++;
            }

            if (servo_isEnable() && servo_isModeSet()) {
                RCLCPP_INFO(node_->get_logger(), "[ServoKeya] servo[%s] enable succeed.", joint_name.c_str());
                motorState = Ready;
                return true;
            }
            else {
                RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] servo[%s] init failed.", joint_name.c_str());   
                motorState = Error;
                return false;
            }
        }
        else
        {
            RCLCPP_ERROR(node_->get_logger(), "[ServoKeya] servo[%s] error[0x%x] is sitll obtained after clear error", joint_name.c_str(), errcode);
            motorState = Error;
            return false;
        }
    }
    return false;
}

int8_t ServoKeya::Motor_GetMode()
{
    return pc_.mode;
}


MotorState ServoKeya::Motor_GetState() 
{
    return motorState;
}

double ServoKeya::Motor_GetCurrent()
{
    return (double)pc_.current;
}

void ServoKeya::errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg)
{
    pc_.error_code = (msg->data & 0xFFFE);
}

void ServoKeya::current_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.current_pos = msg->data;
}

void ServoKeya::current_vel_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg)
{
    pc_.current_vel = msg->data;
}

void ServoKeya::current_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg)
{
    pc_.current = msg->data;
}

void ServoKeya::status_word_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.status_word = msg->data;
    int mode = (msg->data >> 8) & 0xF;
    if (mode == 1) {
        pc_.mode = Profiled_Velocity;
    }
    else if (mode == 3) {
        pc_.mode = Profiled_Position;
    }
    else
        pc_.mode = mode;
}

}